perm filename MOVEC.SAI[SYS,HE] blob
sn#144665 filedate 1975-02-07 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00024 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 BEGIN "MOVE"
C00007 00003
C00011 00004 α SHOW, DWELL
C00013 00005 α SOLVE, DEBIN
C00016 00006 α DEPTH FINDER. Given an image point XF,YF and its rotated position
C00019 00007
C00022 00008
C00023 00009 α SHOWALL - DISPLAY RESULTS IF DEBUGGING OR DISPLAYING
C00029 00010 α GETFIL, SENDVIDEO, COMWAI
C00032 00011 α COMWAI - wait for command COMD to finish (maximum of TICK ticks
C00047 00012 α MACDIF - controls data flow between computers for correlation
C00049 00013
C00051 00014 α SETFEAT - SET FEATURES MANUALLY
C00053 00015 α SUPRES - DELETE DUPLICATE COORDINATES
C00055 00016 α AUTOFEAT - AUTOMATIC FEATURE SELECTION
C00057 00017
C00060 00018
C00063 00019
C00077 00020 α main program begins here
C00080 00021
C00083 00022
C00086 00023
C00089 00024
C00113 ENDMK
C⊗;
BEGIN "MOVE"
REQUIRE "HELIB[1,3]" LIBRARY;
REQUIRE "DPYSUB.HDR[1,PDQ]" SOURCE_FILE;
REQUIRE "INTERF.HDR[SYS,HE]" SOURCE_FILE;
REQUIRE "CORC.REL[SYS,HE]" LOAD_MODULE;
REQUIRE "⊂⊃||" DELIMITERS;
DEFINE CRLF = ⊂'15&'12⊃, TAB = ⊂" "⊃, α = ⊂COMMENT⊃,
ITYPE(FOO) = ⊂OUTSTR("FOO"&" "&CVS(FOO)&" ")⊃,
SAFEX = ⊂SAFE⊃,
SIARRAY = ⊂SAFEX SHORT INTEGER ARRAY⊃,
SRARRAY = ⊂SAFEX REAL ARRAY⊃,
EXT = ⊂EXTERNAL⊃,
REF = ⊂REFERENCE⊃,
IT(X) = ⊂" X="&CVS(X)⊃,
RT(X) = ⊂" X="&CVF(X)⊃,
ERROR(COND,MES) = ⊂ IF COND THEN
BEGIN OUTSTR(MES&CRLF); CONTINUE "MLOOP"; END⊃;
DEFINE MNUM = 10, comment maximum number of images per picture;
GTOLER = 2, comment tolerance for gradient operator;
N = 16, comment window size (square);
HN = 16 DIV 2, comment 1/2 window size;
TTOLER = 9, comment tolerance for feature discrimination*10;
SAMTOL =⊂3.0⊃, comment tolerance for combining features;
CX=⊂19.9⊃,CY=⊂27.9⊃, comment coordinates of table center ;
IN1 = 3, comment picture input channel;
SV = 4, comment maximum vertical shift for correlation;
SH = 6, comment maximum horizontal shift for ";
PIC1 = 1, comment ID of picture 1 for PDP-11;
PIC2 = 2, comment ID of picture 2 for PDP-11;
ARGS = 3, comment ID of arguments for PDP-11;
DBLK = 4, comment ID of data block for PDP-11;
RPAK = 5, comment ID of repack argument block for PDP-11;
FEAT = 6; comment ID of feature argument block for PDP-11;
SHORT REAL LX, LY, LZ, α coordinates of lens center;
ANGLE, α for depth calculation;
TOLER; α tolerance for depth calculation;
SHORT INTERNAL α these are internal for CORC file;
INTEGER MAXI, α number of features;
YFRST,YLST, α limits of portion of first image;
YLEFT,YRIGHT, α needed to process features;
NUM; α estimated upper bound on MAXI;
SIARRAY TIME[1:MNUM], α starting time for processing each image;
STOR1[1:26], α storage array for reading in images;
DPYBUF[1:200], α display buffer;
IFO[1:1]; α dummy argument;
STRING FILE1; α name of picture file;
EXT SHORT INTEGER BITS, LLINE, FLINE, LSIDE, RSIDE, CDEBUG, IWID, TVWORD;
SHORT INTEGER I,J,K,JOBNO,
NUMIMAG, α number of images available/to be used;
IHT, α number of lines in the images;
HSHFT, α maximum horizontal shift in images;
VSHFT, α maximum vertical shift in images;
PICT, α number of current image;
BUFSIZ, α buffer size for whole image;
ARGADR, α PDP-11 address of argument data block;
PIC1AD, α PDP-11 address of first picture block;
PIC2AD, α PDP-11 address of other picture block;
REPAKD, α PDP-11 address of of repack command args;
DATAD, α PDP-11 address of feature data block;
DATA11, α PDP-11 command data word;
ERR11, α PDP-11 command error word;
INTERPOL, α TRUE iff current image is final image;
DEBUG, α TRUE iff debugging;
TDEB, α TRUE iff debugging tracking;
DEB, α TRUE iff debugging MACDIF routine;
DMAC, α TRUE iff debugging correlation operator;
DATMAX, α size of feature data block for PDP-11;
DDVAVL; α TRUE iff DDVID is available;
SHORT SRARRAY DEGREES[1:MNUM], α angle of turntable for each image;
FO[1:1]; α dummy argument;
SRARRAY A[1:3,1:3], α coordinate transformation;
TRAN[1:10,1:3]; α camera calibration output;
PRELOAD_WITH PIC1, PIC2, DBLK, N, N, SH, SV, 0, 0, 0;
SIARRAY COM[1:10]; α correlation command arguments for PDP-11;
PRELOAD_WITH PIC1,0,0,0,0;
SIARRAY REPACK[1:5]; α repack command arguments for PDP-11;
EXT BOOLEAN PROCEDURE CORNER(INTEGER CNT,X,Y;INTEGER ARRAY F;
REAL ARRAY XC,YC);
EXT INTEGER PROCEDURE GIOWD(INTEGER ARRAY BUF);
EXT PROCEDURE PICINI(INTEGER C,F,E,P; REF BOOLEAN FA; INTEGER ARRAY S);
EXT INTEGER PROCEDURE JOBOK(REF INTEGER JOBNO);
EXT BOOLEAN PROCEDURE CANMAIL(INTEGER JOBNO);
EXT PROCEDURE OVERL(INTEGER ARRAY BUF; INTEGER X,M,L,EXP; BOOLEAN CHAN);
EXT PROCEDURE PICRD(REF BOOLEAN FAIL; INTEGER ARRAY STOR);
EXT PROCEDURE DDVID(STRING COM);
EXT PROCEDURE SENDCOM(STRING COM);
α SHOW, DWELL;
α OUTPUT OVERLAY OF FRAME AND WINDOW (UPPER LEFT AT X,Y) FOR PICTURE L
WITH STRING S (OPTIONAL). If XX≠0 then point displayed at
XX, YY else at center of window. Overlay on DDVID channels
iff available;
SIMPLE INTERNAL PROCEDURE SHOW(INTEGER X, Y, L, XX, YY; STRING S);
BEGIN
DPYSET(DPYBUF);
AIVECT(X-LSIDE,Y-FLINE);
RVECT(N-1,0);
RVECT(0,N-1);
RVECT(-N+1,0);
RVECT(0,-N+1);
IF XX THEN APOINT(XX-LSIDE,YY-FLINE) ELSE RPOINT(HN,HN);
DPYPARS;
OVERL(DPYBUF,2,2,L,1,¬DDVAVL);
IF LENGTH(S) THEN
BEGIN
DPYSET(DPYBUF);
DPYBIG(3);
DPYBRT(2);
AIVECT(-512,-50);
DPYSST(S);
DPYOUT(1);
END;
END;
α wait for DDVID to be free, if previously available - returns
TRUE iff it is available and clears DDVID available flag if it
cannot get it;
SIMPLE BOOLEAN PROCEDURE DWELL;
BEGIN INTEGER I;
IF JOBNO<0 THEN DDVAVL←FALSE ELSE
DO BEGIN "WAIT"
FOR I←1 STEP 1 UNTIL 100 DO IF CANMAIL(JOBNO) THEN DONE;
IF ¬DDVAVL∧I>100 THEN RETURN(FALSE);
DDVAVL ← I≤100;
IF ¬DDVAVL THEN
OUTSTR("DDVID NOT ACCEPTING MAIL - TRY AGAIN?");
END "WAIT" UNTIL DDVAVL∨INCHWL≠"Y";
RETURN(DDVAVL);
END;
α SOLVE, DEBIN;
α CONVERT TV COORDINATES CAMX,CAMY TO TABLE COORDS TBLX,TBLY;
SIMPLE PROCEDURE SOLVE(REAL CAMX, CAMY; REFERENCE REAL TBLX, TBLY);
BEGIN REAL Z;
TBLX ← A[1,1]*CAMX + A[1,2]*CAMY + A[1,3];
TBLY ← A[2,1]*CAMX + A[2,2]*CAMY + A[2,3];
Z ← A[3,1]*CAMX + A[3,2]*CAMY + A[3,3];
TBLX ← TBLX/Z;
TBLY ← TBLY/Z;
END;
α general debugging input routine
if DEBX true and CNTR ≠ 0 then decrement count (keeping it ≥ 0)
otherwise, type 'D'. If 'Q' typed back, set DEBX false.
if DEBX then set ODEB to < 'C' typed back>
if a digit was typed, set CNTR to it;
SIMPLE PROCEDURE DEBIN(REF INTEGER DEBX, CNTR, ODEB);
BEGIN INTEGER S;
IF DEBX∧¬CNTR THEN
BEGIN
OUTSTR(" D");
S ← INCHRW;
DEBX ← S≠"Q";
IF DEBX THEN BEGIN IF S="C" THEN ODEB←TRUE END ELSE ODEB←FALSE;
IF "1"≤S≤"9" THEN CNTR←S-"0";
END ELSE CNTR ← (CNTR-1) MAX 0;
END;
α DEPTH FINDER. Given an image point XF,YF and its rotated position
XL,YL, both in TV coords, and the angle of rotation, THETA, return the
table coordinates of the initial point in TX,TY,TZ and the value of
the procedure is the distance from the lens center LX,LY,LZ;
SIMPLE REAL PROCEDURE DEPTH(REAL XF,YF,XL,YL,THETA;REFERENCE REAL TX,TY,TZ);
BEGIN BOOLEAN B1,B2,BACK;
REAL X,Y,M,NN,PX,PY,QX,QY,QF,QN,INCR,A,B,C,AP,BP,CP,D,AA,
BB,CC,DD,X1,X2,AB,CB;
α get the table coordinates of the projection of the two points;
SOLVE(XF,YF,PX,PY);
SOLVE(XL,YL,QX,QY);
IF CDEBUG THEN OUT(1,RT(XF)&RT(YF)&RT(XL)&RT(YL)&CRLF&
RT(PX)&RT(PY)&RT(QX)&RT(QY)&CRLF);
α set limits of X along line QL with increment near end Q, QX;
QN ← LX;
QF ← (PX MIN QX)-2.0;
INCR ← .875;
α calculate coefficients of line equations:
line PL Ax+By+C=0
line QL APx+BPy+CP=0 ;
A ← LY-PY;
B ← PX-LX;
C ← PY*LX-PX*LY;
AP ← LY-QY;
BP ← QX-LX;
CP ← QY*LX-QX*LY;
α now iterate until we find the correct points: X,Y on PL and
M,NN on QL;
DO BEGIN "ITER"
α first get position of M on QL and solve for NN;
M ← (QF-QN)*INCR+QN;
NN ← (-CP-AP*M)/BP;
α now solve for X,Y such that |M,NN-CX,CY|=|X,Y-CX,CY| ;
AB ← A/B;
CB ← C/B;
AA ← 1 + AB↑2;
BB ← 2*(AB*(CB+CY)-CX);
CC ← CB*(CB+2*CY)-M*(M-2*CX)-NN*(NN-2*CY);
α if there are two solutions on the line, use the one
consistent with the direction of rotation of the
turntable;
BACK ← FALSE;
DD ← 2*AA;
D ← BB↑2-2*DD*CC;
IF D<0 THEN IF ABS(QF-QN)>TOLER THEN
BEGIN QN ← M; CONTINUE; END ELSE
BEGIN
OUTSTR(CRLF&"DEPTH - CANNOT SOLVE FOR X,Y");
TX ← TY ← TZ ← 0;
RETURN(0);
END;
D ← SQRT(D);
X1 ← (-BB+D)/DD;
X2 ← (-BB-D)/DD;
IF CDEBUG THEN OUT(1,RT(X1)&RT(X2)&CRLF);
B1 ← PX≤X1≤LX;
B2 ← PX≤X2≤LX;
IF B1∧B2 THEN
BEGIN "TWOSOL"
AA ← ABS(X1-LX);
BB ← ABS(X2-LX);
IF (THETA>0)≡(PY>QY) THEN
X ← IF AA<BB THEN X1 ELSE X2 ELSE
BACK ← X ← IF AA>BB THEN X1 ELSE X2
END "TWOSOL" ELSE
IF B1 THEN X ← X1 ELSE
IF B2 THEN X ← X2 ELSE
BEGIN "NOSOL"
IF X1>LX∨X2>LX THEN
BEGIN "HIGH"
OUTSTR(CRLF&"DEPTH - X BEHIND LENS CENTER");
TX ← TY ← TZ ← 0;
RETURN(0);
END "HIGH";
X ← X1 MAX X2;
END "NOSOL";
IF BACK THEN BACK ← -1;
Y ← (-C-A*X)/B;
IF CDEBUG THEN OUT(1,IT(BACK)&RT(X)&RT(Y)&CRLF);
α calculate ratio |X,Y-M,NN|/|X,Y-CX,CY|
if these are the correct points, ratio=ANGLE
otherwise determine direction to move M and iterate;
D ← ((X-M)↑2+(Y-NN)↑2)/((CX-X)↑2+(CY-Y)↑2);
IF CDEBUG THEN OUT(1,RT(D)&RT(QF)&RT(QN)&RT(M)&CRLF);
IF (D<ANGLE)≡BACK THEN IF QF=M THEN DONE ELSE QF←M ELSE
BEGIN IF QN=M THEN DONE ELSE QN←M; INCR ← .5; END;
END "ITER" UNTIL ABS(QN-QF)<TOLER;
TX ← X;
TY ← Y;
TZ ← LZ*SQRT((((PX-X)↑2+(PY-Y)↑2)/((PX-LX)↑2+(PY-LY)↑2)
+((QX-X)↑2+(QY-Y)↑2)/((QX-LX)↑2+(QY-LY)↑2))/2);
RETURN(SQRT((TX-LX)↑2+(TY-LY)↑2+(TZ-LZ)↑2));
END;
α SHOWALL - DISPLAY RESULTS IF DEBUGGING OR DISPLAYING
given DAT, a PDP-11 command value array - XC and YC, TV coordinate
arrays - TX,TY,TZ, table coordinate arrays - RANG, the distance
array - and flag F, which is TRUE iff arrays other than XC,YC
are valid;
SIMPLE PROCEDURE SHOWALL(SAFEX SHORT INTEGER ARRAY DAT;
SHORT REAL ARRAY XC,YC,TX,TY,TZ,RANG;BOOLEAN F);
BEGIN INTEGER K, I;
DPYSET(DPYBUF);
DPYBRT(7);
DPYBIG(2);
SETFORMAT(0,2);
K ← 3;
α If F false, or good feature, display feature position and, if F
true, the table coordinates, depth, minimum score, offset,
and percent difference between best two scores;
FOR I←1 STEP 1 UNTIL MAXI DO IF ¬F∨DAT[K+4] THEN
BEGIN
APOINT(XC[I]+.5+HN-LSIDE,YC[I]+.5+HN-FLINE);
DPYSST(" "&CVS(I));
IF F THEN OUTSTR(CRLF&CVS(I)&TAB&
CVF(TX[I])&","&CVF(TY[I])&","&CVF(TZ[I])&TAB&
CVF(RANG[I])&TAB&CVS(DAT[K+5])&" MIN"&TAB&
CVS(DAT[K+2]-DAT[K])&","&CVS(DAT[K+3]-DAT[K+1])&TAB&
CVS((DAT[K+6]-DAT[K+5])*100.0/DAT[K+5])&"%");
K ← K+9;
END ELSE K ← K+9;
DPYPARS;
OVERL(DPYBUF,2,2,2,1,¬DDVAVL);
IF ¬F THEN RETURN;
α if F is true, also display point on other image;
DPYSET(DPYBUF);
K ← 3;
FOR I←1 STEP 1 UNTIL MAXI DO IF DAT[K+4] THEN
BEGIN
APOINT(DAT[K+2]+HN-LSIDE,DAT[K+3]+HN-FLINE);
K ← K+9;
END ELSE K←K+9;
DPYPARS;
OVERLAY ← TRUE;
OVERL(DPYBUF,2,2,3,1,¬DDVAVL);
OVERLAY ← FALSE;
END;
α GETFIL, SENDVIDEO, COMWAI;
α This procedure opens file FILE on channel C and puts the
pointer block in STOR1. Returns TRUE if successful.
Sets IHT, NUMIMAG, BUFSIZ and inputs the camera transform
and turntable angles. Tests for invalid picture file;
SIMPLE BOOLEAN PROCEDURE GETFIL(INTEGER C;STRING FILE);
BEGIN "GETFIL" INTEGER FIL, EX, PPN, FAIL, I;
FIL ← CVFIL(FILE,EX, PPN);
PICINI(C,FIL,EX,PPN,FAIL,STOR1);
IF FAIL THEN
BEGIN
OUTSTR("OPEN FAILED FOR "&FILE&CRLF);
RETURN(FALSE);
END;
IF BITS>4 THEN
BEGIN
OUTSTR("CANNOT HANDLE THIS SAMPLE SIZE - "&FILE&CRLF);
RETURN(FALSE);
END;
IHT ← LLINE-FLINE+1;
IF ¬STOR1[16]∨¬STOR1[7]∨¬STOR1[15] THEN
BEGIN
OUTSTR("REQUIRED BLOCKS MISSING FOR "&FILE&CRLF);
RETURN(FALSE);
END;
FOR I←1 STEP 1 UNTIL 10 DO IF ¬STOR1[15+I] THEN DONE;
NUMIMAG ← I-1;
BUFSIZ ← STOR1[16];
ARRCLR(STOR1,0);
STOR1[7] ← LOCATION(TRAN[1,1]);
STOR1[15] ← LOCATION(DEGREES[1]);
PICRD(FAIL,STOR1);
STOR1[15] ← STOR1[7] ← 0;
RETURN(TRUE);
END "GETFIL";
α send the current TV buffer to DDVID with as the left (IMAGE=1
or right image. If left, also erase both images;
SIMPLE PROCEDURE SENDVIDEO(INTEGER IMAGE);
DDVID("F2,2;"&(IF IMAGE=1 THEN "E2;E3;S0;L2" ELSE "L3")&";↔1;→1");
α COMWAI - wait for command COMD to finish (maximum of TICK ticks
returns DATA11 and ERR11. Types messages every TICK
ticks giving number of time outs. Types error message if
wrong command number returned;
SIMPLE PROCEDURE COMWAI(INTEGER TICK, COMD);
BEGIN INTEGER CNT,COMND;
CNT ← 0;
WHILE TRUE DO
BEGIN "WAIT"
IF COMRET(TICK,COMND,DATA11,ERR11) THEN DONE;
CNT ← CNT+1;
OUTSTR("11 TIMED OUT "&CVS(CNT)&CRLF);
END "WAIT";
IF COMD≠COMND THEN OUTSTR("COMMAND= "&CVS(COMD)&CRLF);
END;
α MACDIF - controls data flow between computers for correlation
returns TRUE if PDP-11 running all right;
SIMPLE BOOLEAN PROCEDURE MACDIF(SAFEX INTEGER ARRAY DAT; INTEGER SIZE);
BEGIN "MACDIFF"
INTEGER CNT,CNTR,I,BLK,TBLK,COMD,X,Y,XX,YY,BEST,NBEST;
α finish setting up command arguments;
COM[8] ← SIZE;
COM[9] ← DMAC;
COM[10] ← DEB;
PUTBLK(ARGADR,LOCATION(COM[1]),10,FALSE);
α send command to PDP-11 and wait for response;
TBLK ← CNTR ← 0;
WHILE TRUE DO
BEGIN "CALL"
SNDCOM(5,ARGS);
COMWAI(400,5);
α check for errors;
IF ERR11>0 THEN
BEGIN "ERR"
OUTSTR("ERROR = "&CVS(ERR11)&CRLF);
RETURN(FALSE);
END "ERR";
α get data block and exit unless debugging;
GETBLK(DATAD,LOCATION(DAT[1]),DATMAX,FALSE);
IF ¬ERR11 THEN DONE;
α unpack debugging data;
BLK ← DAT[2];
I ← 3+(BLK-1)*9;
IF TBLK≠BLK THEN
BEGIN
X ← DAT[I];
Y ← DAT[I+1];
TBLK ← BLK;
SHOW(X,Y,2,0,0,NULL);
END;
XX ← DAT[I+2];
YY ← DAT[I+3];
BEST ← DAT[I+5];
NBEST ← DAT[I+6];
IF BEST<0 THEN BEST←BEST LAND '177777;
IF NBEST<0 THEN NBEST←NBEST LAND '177777;
α debugging output after every correlation;
IF ERR11=-1 THEN
BEGIN "DEBUG"
SHOW(XX,YY,3,0,0,"CORREL:"&CVS(BEST)&" "&
CVS(XX-X)&","&CVS(YY-Y));
IF DMAC THEN DMAC←INCHRW≠"Q";
END "DEBUG";
α debugging output after every best correlation;
IF ERR11=-2 THEN
BEGIN "COR"
SHOW(XX,YY,3,0,0,"BEST:"&CVS(XX-X)&","&CVS(YY-Y)&
" CONF="&CVF((NBEST-BEST)*100.0/BEST)&
"% MIN="&CVS(BEST));
DEBIN(DEB,CNTR,DMAC);
IF ¬DEB THEN DMAC ← FALSE;
END "COR";
α set debug flags for future and restart PDP-11;
PUTWRD(ARGADR+16,DMAC);
PUTWRD(ARGADR+18,DEB);
END "CALL";
RETURN(TRUE);
END "MACDIFF";
α SETFEAT - SET FEATURES MANUALLY;
SIMPLE PROCEDURE SETFEAT(SHORT SAFEX REAL ARRAY XC, YC);
BEGIN "SETFEA" SHORT INTEGER I, Q, X, Y;
Q ← N;
X ← (LSIDE+RSIDE) DIV 2;
Y ← (FLINE+LLINE) DIV 2;
YLST ← YRIGHT ← 0;
YFRST ← YLEFT ← 500;
WHILE TRUE DO
BEGIN "LOOP"
SHOW(X,Y,2,0,0,NULL);
I ← INCHRW;
IF I="←" THEN X←(X-Q) MAX LSIDE ELSE
IF I="→" THEN X←(X+Q) MIN (RSIDE-N) ELSE
IF I="↑" THEN Y←(Y-Q) MAX FLINE ELSE
IF I="↓" THEN Y←(Y+Q) MIN (LLINE-N) ELSE
IF I="↔" THEN Q←IF Q=N THEN 2 ELSE N ELSE
IF I="M" THEN
BEGIN "FEAT"
IF MAXI+1≤NUM↑2 THEN MAXI←MAXI+1 ELSE
BEGIN
OUTSTR("TOO MANY FEATURES"&CRLF);
DONE;
END;
XC[MAXI] ← X;
YC[MAXI] ← Y;
IF X<YLEFT THEN YLEFT←X;
IF X>YRIGHT THEN YRIGHT←X;
IF Y<YFRST THEN YFRST ← Y;
IF Y>YLST THEN YLST ← Y;
OUTSTR("FEATURE MARKED"&CRLF);
END "FEAT" ELSE
IF I="E" THEN DONE ELSE OUTSTR("?");
END "LOOP";
END "SETFEA";
α SUPRES - DELETE DUPLICATE COORDINATES;
INTERNAL SIMPLE PROCEDURE SUPRES(SHORT SAFEX REAL ARRAY XC,YC;
REFERENCE INTEGER MAXI);
BEGIN
INTEGER I, J, K;
SHORT REAL XX, YY, XS, YS, CNT, X, Y;
FOR I←1 STEP 1 UNTIL MAXI-1 DO
BEGIN "SUP"
XS ← X ← XC[I];
YS ← Y ← YC[I];
CNT ← 1.0;
J ← I+1;
WHILE J≤MAXI DO
BEGIN "SUPA"
YY ← YC[J];
XX ← XC[J];
IF ABS(XX-X)<SAMTOL∧ABS(YY-Y)<SAMTOL THEN
BEGIN "ACCUM"
XS ← XS+XX;
YS ← YS+YY;
CNT ← CNT+1.0;
FOR K←J+1 STEP 1 UNTIL MAXI DO
BEGIN "SUPB"
XC[K-1]←XC[K];
YC[K-1]←YC[K];
END "SUPB";
MAXI ← MAXI-1;
END "ACCUM" ELSE J ← J+1;
END "SUPA";
XC[I] ← XS/CNT;
YC[I] ← YS/CNT;
END "SUP";
END;
α AUTOFEAT - AUTOMATIC FEATURE SELECTION;
PROCEDURE AUTOFEAT(SHORT SAFEX REAL ARRAY XC,YC);
BEGIN "AUTO"
SHORT INTEGER VARCNT, SOBCNT, PAGENO, SOB, FEATD, I, J, K;
PRELOAD_WITH PIC1,N,N,TTOLER,GTOLER,0,0;
SAFE OWN INTEGER ARRAY FEAPAR[1:7];
PAGENO ← 1;
YLST ← YRIGHT ← VARCNT ← SOBCNT ← 0;
YFRST ← YLEFT ← 500;
α set up debugging flags;
IF DEBUG THEN
BEGIN
OUTSTR("DEBUG VARIANCE?");
DEB ← INCHWL="Y";
OUTSTR("Debug Corner_finder? ");
CDEBUG←INCHWL="Y";
OUTSTR("DEBUG OPERATOR?");
SOB ← INCHWL="Y";
END ELSE SOB←DEB←CDEBUG←FALSE;
IF CDEBUG THEN
BEGIN
OPEN(1,"DSK",0,0,4,I,I,I);
ENTER(1,"MOVE.DBG",I);
END;
α send FEATUR command to PDP-11;
K ← BLKCOM(FEAT,0,7,FEATD);
IF K LAND 1 THEN
BEGIN
OUTSTR("NO ROOM FOR FEATURE BLOCK"&CRLF);
RETURN;
END;
FEAPAR[6] ← DEB;
FEAPAR[7] ← SOB;
PUTBLK(FEATD,LOCATION(FEAPAR[1]),7,FALSE);
SNDCOM(6,FEAT);
K ← CALL(0,"RUNTIM");
α decode response;
WHILE TRUE DO
BEGIN "FIND"
α procedure processes debugging data returned;
SIMPLE BOOLEAN PROCEDURE CHECK;
BEGIN "CHECK"
IF ¬(0<ERR11≤4) THEN
BEGIN "ERRVAR"
OUTSTR("ERROR= "&CVS(ERR11)&CRLF);
RETURN(TRUE);
END "ERRVAR";
IF ERR11=3∧¬VARCNT THEN
BEGIN "VARDEB"
SHOW(GETWRD(DATA11+2),GETWRD(DATA11+4),2,0,0,
"VARIANCE: SCORE = "&CVF(GETWRD(DATA11)/10));
DEBIN(DEB,VARCNT,SOB);
PUTWRD(FEATD+10,DEB);
END "VARDEB" ELSE VARCNT← 0 MAX VARCNT-1;
IF ERR11=4∧¬SOBCNT THEN
BEGIN "SOBDEB"
SHOW(GETWRD(DATA11+12),GETWRD(DATA11+14),2,
GETWRD(DATA11+8),GETWRD(DATA11+10),
"SOBEL: MAG="&CVS(GETWRD(DATA11))&
" DIR="&CVS(GETWRD(DATA11+2))&","&
CVS(GETWRD(DATA11+4))&" - "&
CVS(GETWRD(DATA11+6)));
DEBIN(SOB,SOBCNT,I);
END "SOBDEB" ELSE SOBCNT←0 MAX SOBCNT-1;
PUTWRD(FEATD+12,SOB);
SNDCOM(6,FEAT);
RETURN(FALSE);
END "CHECK";
COMWAI(100,6);
IF ¬ERR11 THEN DONE ELSE IF ¬(1≤ERR11≤2) THEN
IF CHECK THEN DONE ELSE CONTINUE;
α estimate number of features and declare array to hold output
from PDP-11;
I ← GETWRD(DATA11+6)*6;
J ← IF ERR11=2 THEN NUM*6 ELSE I;
BEGIN "SENFET"
SAFEX INTEGER ARRAY FEATR[1:J];
INTEGER PTR;
PTR ← 1;
α fill array with PDP-11 output until we have it all;
WHILE TRUE DO
BEGIN
GETBLK(DATA11+8,LOCATION(FEATR[PTR]),I,TRUE);
SNDCOM(6,FEAT);
IF ERR11=1 THEN DONE;
PTR ← PTR+I;
COMWAI(100,6);
WHILE ¬(0<ERR11≤2) DO IF CHECK THEN RETURN;
I ← GETWRD(DATA11+6)*6;
IF I<0 THEN CONTINUE "FIND";
END;
α call PDP-10 portion of feature extraction code;
IF CDEBUG THEN OUTSTR(CRLF&"PAGE "&CVS(PAGENO)&CRLF);
CORNER((PTR+I) DIV 6,GETWRD(DATA11+2),
GETWRD(DATA11+4),FEATR,XC,YC);
IF CDEBUG THEN OUT(1,'14);
PAGENO ← PAGENO + 1;
END "SENFET";
END "FIND";
IF CDEBUG THEN RELEASE(1);
α delete duplicate feature coordinates and give processing time;
SUPRES(XC,YC,MAXI);
OUTSTR(CRLF&"FEATURE EXTRACTION TIME ="&CVF((CALL(0,"RUNTIM")-K)
/1000.0)&CRLF);
α if debugging, show results and allow user to delete unwanted
features;
IF DEBUG THEN
BEGIN
SHOWALL(IFO,XC,YC,FO,FO,FO,FO,FALSE);
OUTSTR("EDIT? ");
IF INCHWL="Y" THEN WHILE TRUE DO
BEGIN "EDIT" INTEGER S;
OUTSTR("NUMBER TO DELETE IS ");
S ← CVD(INCHWL);
IF ¬S THEN DONE;
IF S>MAXI THEN CONTINUE;
FOR I←S+1 STEP 1 UNTIL MAXI DO
BEGIN XC[I-1] ← XC[I]; YC[I-1] ← YC[I]; END;
MAXI ← MAXI-1;
SHOWALL(IFO,XC,YC,FO,FO,FO,FO,FALSE);
END "EDIT";
END;
α determine limits of image containing features;
FOR I←1 STEP 1 UNTIL MAXI DO
BEGIN REAL X, Y;
X ← XC[I];
Y ← YC[I];
IF X<YLEFT THEN YLEFT←X;
IF X>YRIGHT THEN YRIGHT←X;
IF Y<YFRST THEN YFRST ← Y;
IF Y>YLST THEN YLST ← Y;
END;
END "AUTO";
α main program begins here;
α initialize program, as well as PDP-11;
SETFORMAT(0,2);
DEBUG ← FALSE;
DDVAVL ← TRUE;
INIT11;
BLKCOM(ARGS,0,10,ARGADR);
α outer loop - set debug flags and initialize display as required;
DO BEGIN "OLOOP"
IF JOBOK(JOBNO)≤0 THEN JOBNO ← -1;
OUTSTR("DEBUG?");
I ← INCHWL="Y";
IF DEBUG∧JOBNO≥0 THEN SENDCOM("E2;E3;");
TYPLOC(548,50);
DEBUG ← I;
α middle loop - get file and number of images;
WHILE TRUE DO
BEGIN "MLOOP"
OUTSTR("TYPE NAME OF FILE: ");
FILE1←INCHWL;
IF ¬GETFIL(IN1,FILE1) THEN CONTINUE "MLOOP";
OUTSTR(CVS(NUMIMAG)&" IMAGES AVAILABLE"&CRLF);
α calculate maximum image shifts and upper bound on # of features;
NUM ← ((IWID DIV N)MAX 1)*((IHT DIV N) MAX 1);
HSHFT ← (NUMIMAG-1)*SH+1;
VSHFT ← (NUMIMAG-1)*SV+1;
α declare feature coordinate arrays and read in first image;
BEGIN "INPUT"
SHORT SAFEX INTEGER ARRAY IM1[1:BUFSIZ];
SAFEX SHORT REAL ARRAY XC,YC[1:NUM];
TVWORD ← GIOWD(IM1);
STOR1[16] ← (TVWORD+1) LAND '777777;
PICRD(I,STOR1);
IF DWELL THEN SENDVIDEO(1);
α also get camera transform;
ARRBLT(A[1,1],TRAN[6,1],9);
LX ← TRAN[4,1];
LY ← TRAN[4,2];
LZ ← TRAN[4,3];
α delete PDP-11 data blocks for second picture and feature data
block first (to minimize checkboarding core) and send
first image to PDP-11;
I ← 8+(LLINE-FLINE+1)*((RSIDE-LSIDE+4) DIV 4);
BLKCOM(0,PIC2,J,PIC2AD);
BLKCOM(0,DBLK,J,DATAD);
J ← BLKCOM(PIC1,PIC1,I,PIC1AD);
ERROR(J LAND 1,"NO ROOM FOR PICTURE 1 - NEED "&CVS(I)&" WORDS");
SNDPIC(PIC1AD,FALSE,FLINE,LLINE,LSIDE,RSIDE);
α extract features from first image;
MAXI ← 0;
IF DEBUG THEN OUTSTR("USE VARIANCE?");
IF DEBUG∧INCHWL="N" THEN SETFEAT(XC,YC) ELSE AUTOFEAT(XC,YC);
α repack first image to smallest possible size;
YFRST ← FLINE MAX (YFRST-1);
YLST ← LLINE MIN (YLST+N);
YRIGHT ← (YRIGHT+N) MIN RSIDE;
YLEFT ← (YLEFT-1) MAX LSIDE;
I ← BLKCOM(RPAK,0,5,REPAKD);
ERROR(J LAND 1,"NO ROOM FOR REPACK BLOCK");
REPACK[2] ← YLEFT;
REPACK[3] ← YFRST;
REPACK[4] ← YRIGHT-YLEFT+1;
REPACK[5] ← YLST-YFRST+1;
PUTBLK(REPAKD,LOCATION(REPACK[1]),5,FALSE);
SNDCOM(3,RPAK);
COMWAI(100,3);
ERROR(ERR11,"REPACK ERROR - "&CVS(ERR11));
α calculate how much of the other images we will need (taking
into account the maximum shift of the correlation operator)
and get some 11 core for them;
YFRST ← FLINE MAX (YFRST-VSHFT);
YLST ← LLINE MIN (YLST+VSHFT);
YLEFT ← LSIDE MAX (YLEFT-HSHFT);
YRIGHT ← RSIDE MIN (YRIGHT+HSHFT);
K ← 8+(YLST-YFRST+1)*((YRIGHT-YLEFT+4) DIV 4);
I ← BLKCOM(PIC2,RPAK,K,PIC2AD);
ERROR(I LAND 1,"NO ROOM FOR PICTURE 2");
α set up 11 data block to hold feature information;
DATMAX ← 2+MAXI*9;
J ← BLKCOM(DBLK,0,DATMAX,DATAD);
ERROR(J LAND 1,"NO ROOM FOR DATA BLOCK");
α declare rest of feature arrays and declare and fill
feature data block for 11 - then send;
BEGIN "CORREL"
SAFEX SHORT INTEGER ARRAY DATBLK[1:DATMAX];
SAFEX SHORT REAL ARRAY TX, TY, TZ, RANG[1:MAXI];
SHORT REAL DEG;
DATBLK[1] ← MAXI;
J ← 3;
FOR I←1 STEP 1 UNTIL MAXI DO
BEGIN
DATBLK[J] ← DATBLK[J+2] ← XC[I]+.5;
DATBLK[J+1] ← DATBLK[J+3] ← YC[I]+.5;
DATBLK[J+4] ← TRUE;
DATBLK[J+7] ← DATBLK[J+8] ← 0;
J ← J+9;
END;
PUTBLK(DATAD,LOCATION(DATBLK[1]),DATMAX,FALSE);
IF DEBUG THEN
BEGIN
OUTSTR("DEBUG TRACKING ?");
TDEB ← INCHWL="Y";
END ELSE TDEB ← FALSE;
α inner loop - track features through images;
FOR PICT← 2 STEP 1 UNTIL NUMIMAG DO
BEGIN "ILOOP"
IF ¬(PICT LAND 1) THEN OUTSTR(CRLF);
ITYPE(PICT);
INTERPOL ← PICT = NUMIMAG;
TIME[PICT-1] ← CALL(0,"RUNTIM");
α get next image and send it to PDP-11
set debugging flags and displays;
STOR1[PICT+15] ← STOR1[PICT+14];
STOR1[PICT+14] ← 0;
PICRD(I,STOR1);
IF TDEB THEN
BEGIN
OUTSTR("DEBUG MACDIF?");
DEB←INCHWL="Y";
IF DEB THEN
BEGIN
IF DWELL THEN SENDVIDEO(PICT);
OUTSTR("DEBUG CORRELATION?");
DMAC←INCHWL="Y";
END ELSE DMAC ← FALSE;
END ELSE DEB ← DMAC ← FALSE;
IF INTERPOL∧¬DEB∧DWELL THEN SENDVIDEO(PICT);
SNDPIC(PIC2AD,FALSE,YFRST,YLST,YLEFT,YRIGHT);
α call MACDIF to coordinate the correlations
and output processing times;
IF ¬MACDIF(DATBLK,IF INTERPOL THEN 1 ELSE 2) THEN DONE;
TIME[PICT] ← I ← CALL(0,"RUNTIM");
OUTSTR(TAB&"TIME="&CVF((I-TIME[PICT-1])/1000.0)&TAB);
END "ILOOP";
α all images processed (or MACDIF error)
give total processing time;
OUTSTR(CRLF&"CORRELATION RUN TIME = "&
CVF((TIME[NUMIMAG]-TIME[1])/1000.0)&CRLF);
α get depth tolerance, unpack 11 feature output, and
call depth routine until user is satisfied;
DEG ← DEGREES[NUMIMAG]-DEGREES[1];
ANGLE ← 4*SIND(DEG/2)↑2;
IF DEBUG THEN
BEGIN
OUTSTR("DEBUG DEPTH?");
CDEBUG ← INCHWL="Y";
END ELSE CDEBUG ← FALSE;
IF CDEBUG THEN
BEGIN "CDEBD"
OPEN(1,"DSK",0,0,4,I,I,I);
ENTER(1,"DEPTH.DBG",I);
SETFORMAT(0,4);
OUT(1,CRLF&CRLF&FILE1&CRLF&RT(LX)&RT(LY)&RT(LZ)&RT(DEG)&
RT(ANGLE)&CRLF&CRLF);
END "CDEBD";
DO BEGIN "DEP"
OUTSTR("TOLER=");
FILE1 ← INCHWL;
TOLER ← ABS(REALSCAN(FILE1,I));
J ← CALL(0,"RUNTIM");
K ← 3;
FOR I ← 1 STEP 1 UNTIL MAXI DO
BEGIN "CALC" SHORT REAL X, Y, XX, YY, BS, NS;
IF DATBLK[K+4] THEN
BEGIN "DEPCAL"
X ← DATBLK[K+2];
Y ← DATBLK[K+3];
α interpolate final readings;
XX ← DATBLK[K+7];
YY ← DATBLK[K+8];
BS ← DATBLK[K+5];
NS ← DATBLK[K+6];
α IF CDEBUG THEN OUT(1,IT(I)&RT(X)&RT(Y)&RT(XX)&
RT(YY)&RT(BS)&RT(NS)&CRLF);
IF (NS-BS)<(BS/2) THEN
BEGIN "ADJUST"
NS ← 0 MAX (.5-(NS-BS)/BS);
α X ← X-(X-XX)*NS;
α Y ← Y-(Y-YY)*NS;
END "ADJUST";
RANG[I] ← DEPTH(DATBLK[K],DATBLK[K+1],
X,Y,DEG,TX[I],TY[I],TZ[I]);
IF CDEBUG THEN OUT(1,RT(|RANG[I]|)&RT(|TX[I]|)&
RT(|TY[I]|)&RT(|TZ[I]|)&CRLF);
END "DEPCAL";
K ← K+9;
END "CALC";
OUTSTR(CRLF&"CALCULATION RUN TIME ="&
CVF((CALL(0,"RUNTIM")-J)/1000.0));
SHOWALL(DATBLK,XC,YC,TX,TY,TZ,RANG,TRUE);
OUTSTR(CRLF&"CALCULATE DEPTH AGAIN?");
END "DEP" UNTIL INCHWL≠"Y";
α finished with this picture;
END "CORREL";
END "INPUT";
DONE "MLOOP";
END "MLOOP";
RELEASE(1);
RELEASE(IN1);
OUTSTR("MORE PICTURES?");
END "OLOOP" UNTIL INCHWL≠"Y";
IF DDVAVL∧JOBNO≥0 THEN SENDCOM("E2;E3;");
END "MOVE";